#ifndef _ROBOT_CONTROL_TASK_H_
#define _ROBOT_CONTROL_TASK_H_
#include "stdint.h"

#include "pid.h"
#include "algorism.h"
//#include "adrc.h"

#define CHASSIS_FORWARD_YAW_MACHENICAL_ANGLE  1999//底盘正向朝向时对应的yaw电机机械角
#define GIMBAL_FORWARD_YAW_MACHENICAL_ANGLE 1999//2752 //选定的云台正向朝向时的yaw电机机械角
#define CHASSIS_MOTOR_FRONTFEED_RATIO 2000
#define GIMBAL_PITCH_HORIZON_MACHENICAL_ANGLE 2603
#define SHOULDER0_DEFAULT_MACHENICAL_ANGLE 1960
#define SHOULDER1_DEFAULT_MACHENICAL_ANGLE 0
#define ELBOW0_DEFAULT_MACHENICAL_ANGLE -300
#define ELBOW1_DEFAULT_MACHENICAL_ANGLE 140
#define HIP0_DEFAULT_MACHENICAL_ANGLE 16
#define HIP1_DEFAULT_MACHENICAL_ANGLE 100
#define KNEE0_DEFAULT_MACHENICAL_ANGLE 1500
#define KNEE1_DEFAULT_MACHENICAL_ANGLE 155

#define LIMITTED 0
#define NO_LIMIT 1

#define STIR_STEP 36.0 //拨盘移到下一位置是增加多少度
#define SINGLE_SHOOT_POSITION 27
#define STIR_FRONTFEED 500

#define BULLET_SPEED_UPPER_LIMIT 28.5
#define BULLET_SPEED_LOWER_LIMIT 27.5
#define FRIC_INIT_SPEED 7200
//#define FRIC_READY_SPEED 6000
#define FRIC_UPPER_LIMIT_SPEED 7400

#define EXTRA_CHARGING 1
#define STOP_EXTRA_CHARGING 0

#define I36 36
#define I10 10
#define I9  9


/**
 * @brief 底盘控制相关结构体，存放闭环控制器，目标赋值等等
 */
typedef struct
{
	/*底盘跟随控制相关*/
	struct
	{
		PIDStruct follow_speed_need_pid;		//以差角为pid输入算所需的跟随速度
		int8_t revolve_return_flag;           //用来判断自旋恢复方式的flag
	}ChassisFollowControl;
	/*云台坐标系下的目标速度*/
	struct
	{
		float speed_x_mps;
		float speed_y_mps;
		//加底盘整体速度的反馈控制
		PIDStruct speed_x_compensate_pid;
		PIDStruct speed_y_compensate_pid;		
	}GimbalCoordinateInput;
	/*底盘坐标系下的目标速度*/
	struct
	{
		float speed_x_mps;	// m/s
		float speed_y_mps;	// m/s
		float speed_w_rps;	// rps
	}ChassisCoordinateInput;
	/*底盘实际需要的目标速度，从目标速度赋值过来后，经过功率限制，反馈补偿可能会改变*/
	struct
	{
		float speed_x_mps;	// m/s
		float speed_y_mps;	// m/s
		float speed_w_rps;	// rps
		float power_limit_scale;	//底盘功率限制缩减设定底盘速度系数
		float compensate_power;		//电容补偿功率，可能有正有负，为负时即为惩罚功率，当电容电压过低时可能会考虑
	}ChassisRealNeedInput;
	/*轮电机的目标转速及控制*/
	struct
	{
		float target_speed_mps[4];	// m/s
		int16_t target_motor_output[4];
		PIDStruct speed_control_pid[4];
	}WheelMotorControl;
	/*底盘观测真实数据*/
	struct
	{
		float gimbal_to_chassis_delta_angle_d;	//云台坐标系与底盘坐标系的差角，degree
		float chassis_follow_angle_d;			//底盘跟随云台角
		float wheel_real_speed_mps[4];		//轮速，m/s
		//根据当前轮速解算出的真实底盘速度，m/s
		float speed_x_mps;	// m/s
		float speed_y_mps;	// m/s
		float speed_w_rps;	// rps
	}ChassisEstimate;
	struct
	{
		int16_t total_output_power;
		int16_t state;
		float max_compensate_power;
	}SuperCapacity;
}ChassisControl;

/**
 * @brief 云台控制相关结构体
 */
typedef struct
{
	/*云台目标输入*/
	struct
	{
		float shoulder_angle_d[2];	//期望的肩关节目标角
		float elbow_angle_d[2];		//期望的肘关节目标角
		float hip_angle_d[2];		//期望的髋关节目标角
		float knee_angle_d[2];		//期望的膝关节目标角
	}MotorTargetInput;
	/*云台控制相关*/
	struct
	{
		/*控制器*/
		PIDStruct shoulder_angle_pid[2];	
		PIDStruct elbow_angle_pid[2];
		PIDStruct hip_angle_pid[2];
		PIDStruct knee_angle_pid[2];
		
		PIDStruct shoulder_speed_pid[2];	
		PIDStruct elbow_speed_pid[2];
		PIDStruct hip_speed_pid[2];
		PIDStruct knee_speed_pid[2];
		/*电机实际输出值*/
		int16_t shoulder_target_angle_output[2];
		int16_t elbow_target_angle_output[2];
		int16_t hip_target_angle_output[2];
		int16_t knee_target_angle_output[2];
		
		int16_t shoulder_target_speed_output[2];
		int16_t elbow_target_speed_output[2];
		int16_t hip_target_speed_output[2];
		int16_t knee_target_speed_output[2];
	}MotorControl;
	/*云台真实姿态观测*/
	struct
	{
		float shoulder_angle_d[2];
		float shoulder_angular_velocity_dps[2];
		float shoulder_angular_error_angle_d[2];
//		float robot_slope_angle;//机器人整体倾斜角度
		float elbow_angle_d[2];
		float elbow_angular_velocity_dps[2];	
		float elbow_angular_error_angle_d[2];
		float hip_angle_d[2];
		float hip_angular_velocity_dps[2];
		float hip_angular_error_angle_d[2];
		float knee_angle_d[2];
		float knee_angular_velocity_dps[2];
		float knee_angular_error_angle_d[2];
	}MotorEstimate;
}Control;

/**
 * @brief 发射控制相关结构体
 */
typedef struct
{
	
	struct
	{
		float stir_position_d;
		float bullet_speed_mps[2];
		uint8_t heat_flag;
		uint8_t blocked_time;
		uint8_t	blocked_flag;		
	}ShootEstimate;
	
	struct
	{
		float fric_speed_rpm[2];
		float stir_speed_rps;
		float stir_position_d;
		
		//连发标志位
		int16_t calculated_position_flag;
		
		//单发标志位
		int16_t single_shoot_reset_flag;
		int16_t single_shoot_position_cnt;
		
		//防堵转反转计数器
		uint8_t reversal_cnt;
	}ShootTargetInput;
	
	struct
	{
		PIDStruct fric_speed_pid[2];
		int16_t fric_target_output[2];
		
		PIDStruct stir_speed_control_pid;
		PIDStruct stir_angle_control_pid;
		int32_t stir_speed_target_output;
		float stir_angle_target_output;
	}ShootMotorControl;
		
}ShootControl;

void GimbalPoseUpdate(float pitch_angle, float pitch_angle_w, float yaw_angle, float yaw_angle_w, float roll_angle, float roll_angle_w);
void PIDRefreshBuffer(PIDStruct* const pid);
#endif
